/*
 * Gerade.cpp
 *
 *  Created on: Feb 14, 2011
 *      Author: alki
 */

#include "Gerade.h"
#include "Matrix.h"
#include <stdlib.h>
#include <iostream>
#include "../Exception.h"

using namespace std;

Gerade::Gerade() {
}

Gerade::~Gerade() {
}

bool Gerade::Intersects_E(const Ebene &e,Vektor *S,float *rret,float *sret) const {
	if(e.m_dir1.CrossProduct(e.m_dir2).ScalarProduct(this->m_dir)==0) //parallel
		return NULL;

/*	float m[3][4];
	float n[3][4];
	float o[3][4];

	m[0][0] = e->m_pos->getX()-this->m_pos->getX();
	m[1][0] = e->m_pos->getY()-this->m_pos->getY();
	m[2][0] = e->m_pos->getZ()-this->m_pos->getZ();

	m[0][1] = this->m_dir->getX();
	m[1][1] = this->m_dir->getY();
	m[2][1] = this->m_dir->getZ();

	m[0][2] = e->m_dir1->getX();
	m[1][2] = e->m_dir1->getY();
	m[2][2] = e->m_dir1->getZ();

	m[0][3] = e->m_dir2->getX();
	m[1][3] = e->m_dir2->getY();
	m[2][3] = e->m_dir2->getZ();

	float r = 0;
	float s = 0;
	float t = 0;

	int c=0;
	if(m[0][1]==0) {
		for(int i=0;i<4;i++)
			n[c][i]=m[0][i];
		c++;
	}
	if(m[1][1]==0) {
		for(int i=0;i<4;i++)
			n[c][i]=m[1][i];
		c++;
	}
	if(c<2 && m[2][1]==0) {
		for(int i=0;i<4;i++)
			n[c][i]=m[2][i];
		c++;
	}

	if(c<2) {
		if(m[0][1]!=0) {
			if(c<2 && m[1][1]!=0) {
				for(int i=0;i<4;i++) {
					n[c][i]=m[1][1]*m[0][i]-m[0][1]*m[1][i];
				}
				c++;
			}
			if(c<2 && m[2][1]!=0){
				for(int i=0;i<4;i++) {
					n[c][i]=m[2][1]*m[0][i]-m[0][1]*m[2][i];
				}
				c++;
			}
		}
		if(m[1][1]!=0) {
			if(c<2 && m[2][1]!=0){
				for(int i=0;i<4;i++) {
					n[c][i]=m[2][1]*m[1][i]-m[1][1]*m[2][i];
				}
				c++;
			}
		}
	}
	if(m[0][1]!=0) {
		for(int i=0;i<4;i++) {
			n[2][i]=m[0][i];
			o[2][i]=m[0][i];
		}
	}
	if(m[1][1]!=0) {
		for(int i=0;i<4;i++) {
			n[2][i]=m[1][i];
			o[2][i]=m[1][i];
		}
	}
	if(m[2][1]!=0) {
		for(int i=0;i<4;i++) {
			n[2][i]=m[2][i];
			o[2][i]=m[2][i];
		}
	}

	c=0;
	if(n[0][2]==0) {
		for(int i=0;i<4;i++) {
			o[0][i]=n[0][i];
			o[1][i]=n[1][i];
		}
		c++;
	}
	if(n[1][2]==0) {
		for(int i=0;i<4;i++) {
			o[0][i]=n[1][i];
			o[1][i]=n[0][i];
		}
		c++;
	}
	if(c<1) {
		for(int i=0;i<4;i++) {
			o[0][i]=n[0][2]*n[1][i]-n[1][2]*n[0][i];
			o[1][i]=n[1][i];
		}
	}

	if(m[1][1]!=0) {
		for(int i=0;i<4;i++) {
			m[1][i]=m[2][1]*m[1][i]-m[1][1]*m[2][i];
		}
	}
	else if(m[0][1]!=0) {
		for(int i=0;i<4;i++) {
			m[1][i]=m[2][1]*m[0][i]-m[0][1]*m[2][i];
		}
	}

	if(m[0][2]!=0)
	for(int i=0;i<4;i++) {
		m[0][i]=m[1][2]*m[0][i]-m[0][2]*m[1][i];
	}

	t=-o[0][0]/o[0][3];
	s=-(o[1][0]+o[1][3]*t)/o[1][2];
	r=(o[2][0]+o[2][3]*t+o[2][2]*s)/o[2][1];/*/

	Matrix mat;
	mat.a_11=e.m_dir1.getX();
	mat.a_21=e.m_dir1.getY();
	mat.a_31=e.m_dir1.getZ();
	mat.a_12=e.m_dir2.getX();
	mat.a_22=e.m_dir2.getY();
	mat.a_32=e.m_dir2.getZ();
	mat.a_13=-this->m_dir.getX();
	mat.a_23=-this->m_dir.getY();
	mat.a_33=-this->m_dir.getZ();

	Matrix matinv=mat.Inv();
	Vektor P=this->m_pos-e.m_pos;
	Vektor RS=matinv*P;

	float s=RS.getX();
	float t=RS.getY();
	float r=RS.getZ();/**/

	if(fabs(r)<0.0001 && r>0) r=0;
	if(fabs(s)<0.0001 && s>0) s=0;
	if(fabs(t)<0.0001 && t>0) t=0;

	if(rret!=NULL) *rret=s;
	if(sret!=NULL) *sret=t;

	if(S!=NULL) {
		S->setX(this->m_pos.getX()+r*this->m_dir.getX());
		S->setY(this->m_pos.getY()+r*this->m_dir.getY());
		S->setZ(this->m_pos.getZ()+r*this->m_dir.getZ());
	}

	return true;
}

void Gerade::CalcHelpers() {
	this->i=this->m_dir.getX();
	this->j=this->m_dir.getY();
	this->k=this->m_dir.getZ();

	this->ii = 1.0f/this->i;
	this->ij = 1.0f/this->j;
	this->ik = 1.0f/this->k;

	//ray slope
	this->ibyj = this->i * this->ij;
	this->jbyi = this->j * this->ii;
	this->jbyk = this->j * this->ik;
	this->kbyj = this->k * this->ij;
	this->ibyk = this->i * this->ik;
	this->kbyi = this->k * this->ii;
	this->c_xy = this->m_pos.getX() - this->jbyi * this->m_pos.getX();
	this->c_xz = this->m_pos.getZ() - this->kbyi * this->m_pos.getX();
	this->c_yx = this->m_pos.getX() - this->ibyj * this->m_pos.getY();
	this->c_yz = this->m_pos.getZ() - this->kbyj * this->m_pos.getY();
	this->c_zx = this->m_pos.getX() - this->ibyk * this->m_pos.getZ();
	this->c_zy = this->m_pos.getY() - this->jbyk * this->m_pos.getZ();

	if(i < 0) {
		if(j < 0) {
			if(k < 0)
				this->classification = MMM;
			else if(k > 0)
				this->classification = MMP;
			else
				this->classification = MMO;
		}
		else {
			if(k < 0) {
				this->classification = MPM;
				if(j==0)
					this->classification = MOM;
			}
			else {
				if((j==0) && (k==0))
					this->classification = MOO;
				else if(k==0)
					this->classification = MPO;
				else if(j==0)
					this->classification = MOP;
				else
					this->classification = MPP;
			}
		}
	}
	else {
		if(j < 0) {
			if(k < 0) {
				this->classification = PMM;
				if(i==0)
					this->classification = OMM;
			}
			else {
				if((i==0) && (k==0))
					this->classification = OMO;
				else if(k==0)
					this->classification = PMO;
				else if(i==0)
					this->classification = OMP;
				else
					this->classification = PMP;
			}
		}
		else {
			if(k < 0) {
				if((i==0) && (j==0))
					this->classification = OOM;
				else if(i==0)
					this->classification = OPM;
				else if(j==0)
					this->classification = POM;
				else
					this->classification = PPM;
			}
			else {
				if(i==0) {
					if(j==0)
						this->classification = OOP;
					else if(k==0)
						this->classification = OPO;
					else
						this->classification = OPP;
				}
				else {
					if((j==0) && (k==0))
						this->classification = POO;
					else if(j==0)
						this->classification = POP;
					else if(k==0)
						this->classification = PPO;
					else
						this->classification = PPP;
				}
			}
		}
	}
}
